Pose Estimation using Sensors

ABSTRACT

A method and system estimates a three-dimensional (3D) pose of a sensor by first acquiring scene data of a 3D scene by the sensor. Two-dimensional (2D) lines are detected in the scene data and the 2D lines are matched to 3D lines of a 3D model of the scene to produce matching lines. Then, the 3D pose of the sensor is estimated using the matching lines.

FIELD OF THE INVENTION

The invention is generally related to computer vision, and more particularly to estimating a pose of a sensor.

BACKGROUND OF THE INVENTION

Conventional cameras generally observe a scene as a sequence of images acquired at a fixed frame rate. This image-based representation is an inefficient and redundant encoding of the scene that wastes power, memory, and time. Because the exposure time for each image is the same, this can also lead to difficulties in imaging scenes that have a high dynamic range, i.e., scenes containing very dark and very bright regions.

Dynamic vision sensors (DVS) sensors are different from conventional cameras. DVS do not output actual pixel intensity values. Instead, DVS signal pixel-level changes, i.e., whether the intensity of a pixel increases or decreases. DVS are inspired by a model of the human retina, see Delbruck et al., “A silicon early visual system as a model animal,” Vision Research, 2004. These sensors do not send entire images at fixed frame rates. Instead, DVS only send pixel-level changes as 0 and 1 binary events, similar to signals from retinal ganglion cells, at exactly the time the changes occur. For example, a 0 indicates a decrease in intensity and a 1 indicates an increase in intensity of a particular pixel. As a result, the events are transmitted at microsecond time resolution, equivalent to or better than conventional high-speed camera running at thousands of frames per second. That is, instead of wastefully sending entire images at fixed frame rates, in applications of concern to the invention, the DVS only transmits the local pixel-level changes caused by movement of the sensor at the time the events occur.

In U.S. 20150160737, “Apparatus and method for recognizing gesture using sensor,” 2015, a dynamic vision sensor (DVS) is used for gesture recognition. In U.S. 20140354537, “Apparatus and method for processing user input using motion of object,” 2014, a DVS is used for tracking moving objects. Conventional procedures, such as optical flow have been developed for DVS, see EP 2795580, “Method of estimating optical flow on the basis of an asynchronous light sensor,” 2014.

Some methods extend visual odometry to DVS using a conventional camera, see Censi et al., “Low-latency event-based visual odometry,” ICRA, 2014. Censi et al., disclose a visual odometry procedure used with a DVS and a complementary metal-oxide semiconductor (CMOS) camera. A novel calibration procedure performs spatio-temporal calibration of the DVS and the CMOS camera. Their approach estimates the relative motion of the DVS events with respect to a previous CMOS frame. The method is accurate for rotation estimates, whereas translation measurements tend to be noisy.

SLAM techniques and 3D reconstruction methods have also been developed for DVS, see Weikersdorfer et al., “Simultaneous localization and mapping for event-based vision systems,” Computer Vision Systems, 2013, and Carneiro et al., “Event-based 3D reconstruction from neuromorphic retinas,” Neural Networks, 2013. The Lucas-Kanade tracking procedure for optical flow estimation has been extended for the DVS. Certain registration methods, such as iterative-closest-point (ICP), have been extended for DVS to control micro-grippers. DVS can also be useful for evasive maneuvering of quadrotors. A pair of DVS can be used as a stereo camera for reconstructing objects near a quadrotor for predicting and avoiding collisions.

In the case of asynchronous DVS, the concept of epipolar geometry cannot be directly used. Epipolar geometry provides a relationship between corresponding points in a pair of images acquired from different viewpoints. Recently, a general version of epipolar geometry applicable for asynchronous event-based stereo-configuration has been developed, see Benosman et al., “Asynchronous event-based Hebbian epipolar geometry,” IEEE Trans. Neural Network, 2011.

The stereo-matching can be done using event histograms and estimated depths can be used for gesture recognition. Beyond stereo, some methods use more than two event sensors for 3D reconstruction.

DVS-based solutions are also known for particle-filter based localization and mapping. It is possible to build a mosaic of a scene solely using a DVS without any additional sensors. This is achieved by tracking the sensor motion and using the estimated motion for registering and integrating the data.

However, as a drawback, the cost of a typical DVS is in the range of several thousand dollars.

SUMMARY OF THE INVENTION

The embodiments of the invention provide a method and system for localization and tracking using a sensor acquiring scene data. The invention provides two main embodiments. For low speed applications and a low cost system, the sensor can be a conventional camera. For high speed application, a dynamic visual sensor (DVS) can be used, although the cost of such a sensor can be two orders of magnitude higher than that of a camera, which can be below $20.

With the DVS, events are usually signaled for pixels with large temporal gradients. The temporal gradients are typically generated by edges in the scene (high spatial gradients) as the sensor moves with respect to the scene. The embodiments use a fast spatio-temporal binning scheme to detect 2D lines from these events associated with vertical edges. A 3D model of the scene contains 3D vertical edges associated with some of the 2D lines observed in the scene data. Establishing the correct 2D line to 3D edge correspondence allows localization of the sensor to the 3D model.

The embodiments provide an accurate absolute pose estimation method using a sensor. In particular, we represent a real world scene with an edge-based 3D model and localize the sensor by matching the 2D lines obtained from the scene data with the 3D lines of the 3D scene model.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 is a schematic of a method and system for estimating a pose of a sensor according to embodiments of the invention;

FIG. 2 is a schematic of sensor alignment according to embodiments of the invention;

FIG. 3 is a schematic of spatio-temporal binning according to embodiments of the invention; and

FIG. 4 is a schematic of a projection of three 3D points to three 2D points on a plane for the task of estimating the pose.

DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

The embodiments of our invention provide a method and system for estimating a pose of a sensor. The sensor can be either camera or an event sensor, such as a digital vision sensor, which only signals events when pixels sense a change in intensity, either a decrease or an increase. Such pixels are usually associated with large gradients, such as edges in the world as the sensor is moved.

In order to achieve a fast response, the invention focusses on real world scenes with strong features corresponding to, e.g., 3D vertical lines. The method can detect the vertical lines using a spatio-temporal binning scheme.

The pose estimation method uses correspondences between (2D) lines obtained from the scene data and three-dimensional (3D) lines in the world as represented in a scene model.

An inertial measurement unit (IMU) measures absolute roll and pitch angles to align the scene data with the vertical direction in the real world scene.

To the best of our knowledge, the vertical line-based pose estimation problem described herein has not been solved in the context of conventional frame-based sensors and cameras, and digital vision sensors.

Dynamic Vision Sensor

An example event sensor is an iniLabs DAVIS 240B dynamic vision sensor (DVS). This sensor includes an active pixel sensor (APS) circuit, (DVS) circuits, and an IMU on a single board. The APS and DVS circuits within a pixel share a single photodiode. The resolution of the APS frame is 240×180 pixels and dimensions of the scene data are 190×180. The APS frame rate is 23 Hz, and the IMU operates at 2300 Hz. The IMU provides alignment data, e.g., acceleration and angular velocity in six dimensions.The APS data can be used to visualize the results. We rely on the IMU data to align the scene data with the vertical direction in the real world scene.

The DVS signals pixel-level intensity changes using binary states. Each pixel asynchronously emits an ON event if a log-compressed intensity increases by a predetermined amount, and an OFF event when the intensity decreases. An event E is noted as tuple <x, y, t, s > where (x, y) is the pixel location, t is the time, and s is the event state: 1 for ON and 0 for OFF. The sensor can generate on the order of a million events per second.

It is understood that the sensor can be a conventional camera, or video camera because the camera can emulate the event sensor, albeit at the slower frame rate of the camera. Such an emulation can be performed by taking a difference between successive frames and classifying the difference at each pixel as an event or non-event based on a magnitude of the difference. Differences within some threshold distance from 0 are considered non-events. Differences that are greater than the threshold are positive events and are coded as <x, y, 1>, where (x, y) are the pixels coordinates. Differences that are smaller than the threshold are coded as <x, y, 0>. Since the camera frame rate is relatively slow compared to the frame rate of the DVS, events are naturally grouped on a frame by frame basis and there is no need to retain the temporal variable of the frame number, although this can be done for a complete analogy to the DVS.

Pose Estimation Method

As shown in FIG. 1, the method concurrently acquires 110 data 101 that includes scene data (x, y, t, s) of a scene 102 by the event sensor 103 and alignment data by the IMU 107. The alignment data can be used to determine roll and pitch of the event sensor 103 with respect to the scene. The scene data are aligned 120 to a vertical direction 106 of the scene to produce aligned scene data using the alignment data. Then, 2D vertical lines are detected 130 in the aligned scene data. The detecting fits the 2D lines in a spatial domain by fitting planes to the events data in a spatio-temporal domain.

The 2D vertical lines are matched 140 to vertical lines in the 3D model of the scene to produce matching lines. The pose 105 can then be estimated 150 from the matching lines. The steps, which can be performed in a processor 100 connected to the sensor and memory as known in the art, can be repeated while the sensor moves 104 to different viewpoints and poses. In the case wherein the sensor is moving, the method estimates a trajectory of poses for the event sensor at multiple viewpoints. The trajectory can be smoothed with a Kalman filter.

We use vertical lines as the feature to estimate the pose 105 of the 103 sensor given a 3D (world) model of the scene. By ignoring height, the 6 degree-of-freedom (DOF) localization problem is simplified to a 3 DOF problem. The 3 DOF (planar location and yaw) is more important in localization of vehicles and mobile robots, while the remaining variables (height, roll, pitch) can be obtained using the IMU, and other ways. Detecting only the vertical lines also make the feature extraction much faster than finding all lines with different orientations.

Sensor Alignment using IMU

The IMU alignment data are used to align 120 the scene data to a vertical direction in the (world) scene. This ensures that the sensed 2D lines correspond to real vertical lines in the scene, regardless of the sensor's pose. The IMU gyroscope can have large and time-dependent drift. Therefore, we apply an attitude heading reference (AHRS) procedure to obtain the sensor's attitude in the world coordinate system, see Mahony et al., “Nonlinear complementary filters on the special orthogonal group,” Automatic Control, IEEE Transactions on, 53(5):1203-1218, 2008. Because the accelerometer only measures the gravity vector, the IMU cannot obtain a precise yaw angle. Therefore, we only use the roll and pitch angles from the IMU.

Based on the attitude measured by IMU, we use R to denote a rotation offset to align the sensor's vertical view and the world's vertical direction. The relation between the pixel p′ (x′, y′) in the aligned scene data and a location p(x, y) in the acquired scene data is

$\begin{matrix} {{\begin{pmatrix} x^{\prime} \\ y^{\prime} \\ 1 \end{pmatrix} = {{sKRK}^{- 1}\begin{pmatrix} x \\ y \\ 1 \end{pmatrix}}},} & (1) \end{matrix}$

where K is a matrix of intrinsic parameters for the sensor, and s is a scale factor that denotes that the relation is up to a scale.

FIG. 2 shows the alignment diagramatically. On the left, we show the sensor acquiring an image of a cube 210. The 3D point P (X₁, Y₁, Z₁) 220 is projected on to an image pixel p(x₁, y₁) 230. We use the cube to denote that the world coordinate system, with respect to which the sensor, is slightly rotated. Using the IMU, we determine the rotation offset R between the camera coordinate system (with an origin O_(c,)) and the world coordinate systems (with an origin O_(w,)) on the roll and pitch axes. On the right, we show the image correction using the rotation offset R and the calibration matrix. Let p (x₁, y₁) and p′ (x′₁, y′₁) denote the pixels before and after the alignment, respectively. As can be seen, two pixels p′ (x′₁, y′₁) and q′ (x′₂, y′₂) are aligned on the vertical axis 240 having x′₁=x′₂ after the alignment.

Line-Detection using Spatio-Temporal Binning

We use a voting procedure based on pixel locations of the events and the associated times to detect 130 lines in the aligned scene data using a spatio-temporal binning procedure. The procedure is based on the fact that pixels on the same vertical line or edge have similar x coordinate, and similar intensity contrast.

As shown in FIG. 3, we use a thin strip bin V_(dx) (x_(i)) 310 to represent a rectangular area of pixels from (x_(i), 1) to (x_(i)+dx, y_(max)). The intensity contrast of an edge in the world is closely related to a time interval when multiple events are signaled for the same pixel. This is because the same world edge causes multiple events at the same pixel during sensor motion, and thus pixels on high-contrast edges cause more events within the same time interval. This implies that the time interval Δt between two events signaled for the same location is shorter for high-constrast edges. We use a bin V_(dx,dt) (x_(i), Δt_(j)) to denote the pixels that are within V_(dx) (x_(i)) and has a signaling interval between Δt_(j) and Δt_(j)+dt.

As shown in FIG. 3, we preset overlapping sets of spatial intervals, and we consider a bin to contain a line when the bin has a number of events greater than a predetermined threshold, which is a local maximum with respect to nearby bins. In particular, we ensure that we select a bin V_(dx,dt) (X_(i), Δt_(j)) when the following conditions are satisfied:

-   -   1. Spatial neighboring pixels have fewer votes as given by the         following constraints:

V_(dx,dt) (x_(i), Δt_(j))>V_(dx,dt) (x_(i±1), Δt_(j))+T₁, and

V_(dx,dt) (x_(i), Δt_(j))>V_(dx,dt) (x_(i±2), Δt_(j))+T₂.

-   -   2. There is no other bin V_(dx,dt) (x_(i), Δt_(j)) satisfying         the above threshold conditions when Δt_(k)<Δt_(k).

After the bins are selected, we use least square procedure to fit the lines with the events in the bins, to account for any error in IMU-based alignment.

Pose Estimation using Vertical Lines

We make use of the IMU to obtain the vertical real world direction 106, and thus solve a 3-point pose estimation problem for recovering three unknown motion parameters, i.e., two translation parameters and one rotational angle. After we recover the 3 DOF, we have the five unknown motion parameters out of the total of six unknowns. If the sixth parameter (height=Y) is required, we can easily recover the height using any non-vertical line.

The focus of the invention is to extract the three important motion parameters (X, Z, and yaw) that enable us to localize and track the sensor with respect to a scene with dominant vertical structures.

Line Projections

FIG. 4 schematically shows the projection of three 3D points to three 2D points on a plane. On the left, we show the projection of lines from the cube 210 to an image plane 410. On the right, we show the top view of the projection where vertical lines in the scene are denoted by 2D points (X₁, Z₁), (X₂, Z₂), (X₃, Z₃), and vertical lines in the image (scene data) are denoted by 2D points (x₁, z₁), (x₂, z₂), (x₃, z₃) and the origin is denoted by O.

The 3D points in the world coordinate system (X₁ ^(w), Z₁ ^(w)), (X₂ ^(w), Z₂ ^(w)), (X₃ ^(w), Z₃ ^(w)) can be transferred to the sensor coordinate system using a transformation (R, t).

As shown in FIG. 4, the transformation between a vertical line in the scene and the corresponding 2D line in the image (scene data) is given by

$\begin{matrix} {\begin{pmatrix} x_{i} \\ z_{i} \end{pmatrix} = {{s_{i}\begin{pmatrix} {\cos \; \theta} & {{- \sin}\; \theta} & T_{x} \\ {\sin \; \theta} & {\cos \; \theta} & T_{z} \end{pmatrix}}{\begin{pmatrix} X_{i}^{w} \\ Z_{i}^{w} \\ 1 \end{pmatrix}.}}} & (2) \end{matrix}$

We rearrange Eqn. (2), eliminate the scale variable s_(i) and stack the equations for three 3D to 2D correspondences to obtain a linear system AX=0, where A and X are

$\begin{matrix} {{A = \begin{pmatrix} z_{1} & {- x_{1}} & \left( {{{- z_{1}}Z_{1}^{w}} - {x_{1}X_{1}^{w}}} \right) & \left( {{z_{1}X_{1}^{w}} - {x_{1}Z_{1}^{w}}} \right) \\ z_{2} & {- x_{2}} & \left( {{{- z_{2}}Z_{2}^{w}} - {x_{2}X_{2}^{w}}} \right) & \left( {{z_{2}X_{2}^{w}} - {x_{2}Z_{2}^{w}}} \right) \\ \; & \; & \vdots & \; \end{pmatrix}},{and}} & (3) \\ {X = {\begin{pmatrix} T_{x} \\ T_{z} \\ {\sin \; \theta} \\ {\cos \; \theta} \end{pmatrix}.}} & (4) \end{matrix}$

With three lines, the solution typically is unique. Using the reduced row-echelon form, we obtain the following linear system from the above equation:

$\begin{matrix} {{{\begin{pmatrix} 1 & 0 & 0 & \alpha \\ 0 & 1 & 0 & \beta \\ 0 & 0 & 1 & \gamma \end{pmatrix}\begin{pmatrix} T_{x} \\ T_{z} \\ {\sin \; \theta} \\ {\cos \; \theta} \end{pmatrix}} = \begin{pmatrix} 0 \\ 0 \\ 0 \\ 0 \end{pmatrix}},} & (5) \end{matrix}$

where α, β and γ are constants obtained while determining the reduced row echelon form for the matrix A.

Using the above linear system, we determine the three unknown motion parameters θ, T_(x) and T_(z) as follows:

θ=tan⁻¹(−γ),

T _(x)=−α cos(tan⁻¹(−γ)),

T _(z)=−β cos(tan⁻¹(−γ)).

Given a set of n 2D to 3D line correspondences, where n is greater than or equal to three, we can form a linear system AX=0 using all correspondences and solve the motion variables using a least squares procedure.

Our method uses two modules. When we do not have a reliable previous pose, we localize the sensor using a complete search for 2D to 3D line correspondences. When we have a previous pose, the 2D to 3D line correspondences are known, and we only need to update the position for small changes. The complete search for localization takes more time than the update.

As described above, a conventional camera can emulate a DVS. However such an emulation is not necessary for application of our method. Lines can be extracted directly from a single camera image by 2D edge extraction, and the restriction to vertical lines on the basis of IMU data allows fast pose estimation.

Correspondence Search

The pose estimation requires 2D to 3D line correspondences. The correspondence search is very difficult because of many factors, such as repetitive scene geometry, noisy scene data, missing and spurious lines in the line detection, and incorrect modeling of the world. One obvious solution is to consider all possible matches and pick the best matches based on some error criteria. However, this strategy can be fail even for as few as 20 lines.

In order to solve the search problem, we use a grid-search method that uses constraints based on an angle between viewing directions of lines. In this case, searching the best correspondence means finding a largest set of correspondences with a minimal matching cost determined using reprojection errors for all the line correspondences.

Reprojection Error

Using the estimated poses, we reproject the 3D lines to the image and measure the reprojection error, i.e., a distance between the pixel associated with the event and the reprojected 3D line. The reprojection error for the pose estimation is generally less than two pixels, indicating that the pose estimates are accurate.

Although the invention has been described by way of examples of preferred embodiments, it is to be understood that various other adaptations and modifications can be made within the spirit and scope of the invention. Therefore, it is the object of the appended claims to cover all such variations and modifications as come within the true spirit and scope of the invention. 

We claim:
 1. A method for estimating a three-dimensional (3D) pose of a sensor, comprising steps: acquiring scene data of a 3D scene by the sensor; detecting two-dimensional (2D) lines in the scene data; matching the 2D lines to 3D lines of a 3D model of the scene to produce matching lines; and estimating the 3D pose of the sensor using the matching lines, wherein the sensor is a camera, and the 2D lines and the 3D are vertical lines, and wherein the steps are performed in a processor.
 2. The method of claim 1, further comprising: acquiring alignment data for the sensor using an inertial measuring unit (IMU), wherein the alignment data includes roll and pitch angles of the sensor; and aligning the scene data to a vertical direction of the scene using the alignment data.
 3. The method of claim 1, wherein the scene data includes pixels, and wherein the detecting further comprises: fitting the 2D lines in a spatial domain to the pixels with high spatial gradients.
 4. The method of claim 1, further comprising: applying an attitude heading reference (AHRS) procedure to obtain an attitude of the sensor in a world coordinate system.
 5. The method of claim 2, further comprising: determining a rotation offset R from the alignment data and wherein a relation between a pixel p′ (x′, y′) in the scene data after the aligning and a location p (x, y) in the scene data is ${\begin{pmatrix} x^{\prime} \\ y^{\prime} \\ 1 \end{pmatrix} = {{sKRK}^{- 1}\begin{pmatrix} x \\ y \\ 1 \end{pmatrix}}},$ where K is a matrix of intrinsic parameters for the event sensor, and s is a scale factor that denotes that the relation is up to a scale.
 6. The method of claim 1, wherein detecting the 2D lines uses spatio-temporal binning and a voting procedure.
 7. The method of claim 3, wherein the 2D lines are fitted using a least square procedure.
 8. The method of claim 1, wherein a minimum of three 2D lines and three 3D lines are used for the estimating.
 9. A method for estimating a three-dimensional (3D) pose of a sensor, comprising steps: acquiring scene data of a 3D scene by the sensor; detecting two-dimensional (2D) lines in the scene data; matching the 2D lines to 3D lines of a 3D model of the scene to produce matching lines; and estimating the 3D pose of the sensor using the matching lines, wherein the sensor is a digital event sensor and the 3D lines are vertical, and wherein the steps are performed in a processor.
 10. The method of claim 9, further comprising: acquiring alignment data for the sensor using an inertial measuring unit (IMU), wherein the alignment data includes roll and pitch angles of the sensor; and aligning the scene data to a vertical direction of the scene using the alignment data.
 11. The method of claim 9, wherein the detecting further comprises: fitting the 2D lines in a spatial domain by fitting planes to the scene data in a spatio-temporal domain.
 12. The method of claim 9, wherein detecting the 2D lines uses spatio-temporal binning and a voting procedure.
 13. The method of claim 9, wherein the event data are <x, y, t, s> where (x, y) is a location of a pixel in the scene data, t is a time, and s is a state, where 0 indicates a decrease in an intensity of the pixel and 1 indicates an increase in the intensity of the pixel.
 14. A system for estimating a three-dimensional (3D) pose, comprising: a sensor configured to acquire scene data of a 3D scene; a processor, connected to the sensor, configured to detect two-dimensional (2D) lines in the scene data, and to match the 2D lines to 3D lines of a 3D model of the scene to produce matching lines, and to estimate the 3D pose of the sensor using the matching lines, wherein the sensor is a camera and the 2D lines and the 3D are vertical lines.
 15. The system of claim 12, further comprising: an internal measuring unit (IMU) configured to acquire alignment data for the sensor, wherein the alignment data includes roll and pitch angles of the sensor.
 16. The method of claim 14, wherein the scene data are aligned to a vertical direction of the scene using the alignment data.
 17. The system of claim 14, wherein the sensor is a digital vision sensor. 